package websocket

import (
	"fmt"
	"iron_man_comm/src/common/log"
	"iron_man_comm/src/goroslib"
	"iron_man_comm/src/goroslib/dns"
	"iron_man_comm/src/goroslib/pkg/apimaster"
	"iron_man_comm/src/goroslib/pkg/msgs/std_msgs"
	"iron_man_comm/src/msg/common_msg"
	"time"
)

func (s *IotServer) createRosNode() error {
	dns.GetDNSCacheInterface().UpdateDNSMapping("rk3562-buildroot", s.MachineHost)
	var err error
	masterAddress := fmt.Sprintf("%s:11311", s.MachineHost)
	log.Info("master address: %s", masterAddress)
	s.node, err = goroslib.NewNode(goroslib.NodeConf{
		MasterAddress: masterAddress,
		Name:          fmt.Sprintf("test_node_%d", time.Now().Nanosecond()),
	})
	if err != nil {
		log.Error("create ros node fail, err: %s", err.Error())
		return err
	}
	//iot publish
	for _, topicName := range IOT_PUBLISHER_TOPIC {
		err = s.createPublisher(topicName)
		if err != nil {
			log.Error("create topic %s publisher fail, err: %s", topicName, err.Error())
			return err
		} else {
			log.Info("create topic %s publisher pass", topicName)
		}
	}
	//iot subscriber
	for _, topicName := range IOT_SUBSCRIBER_TOPIC {
		err = s.createSubscriber(topicName)
		if err != nil {
			log.Error("create topic %s subscriber fail, err: %s", topicName, err.Error())
		} else {
			log.Info("create topic %s subscriber pass", topicName)
		}
	}
	//下视信号
	err = s.createDownViewSignal()
	if err != nil {
		log.Error("create down view signal subscriber fail, err: %s", err.Error())
	} else {
		log.Info("create down view signal subscriber pass")
	}
	//下视编码值
	err = s.createDownViewCode()
	if err != nil {
		log.Error("create down view code subscriber fail, err: %s", err.Error())
	} else {
		log.Info("create down view code subscriber pass")
	}
	//撞板
	err = s.createBumpSignal()
	if err != nil {
		log.Error("create bump signal subscriber fail, err: %s", err.Error())
	} else {
		log.Info("create bump signal subscriber pass")
	}

	//悬空
	err = s.createHanging()
	if err != nil {
		log.Error("create hanging subscriber fail, err: %s", err.Error())
	} else {
		log.Info("create hanging subscriber pass")
	}
	//外设在位信号
	err = s.createInstallSignal()
	if err != nil {
		log.Error("create install signal subscriber fail, err: %s", err.Error())
	} else {
		log.Info("create install signal subscriber pass")
	}
	//外设到位信号
	err = s.createReachSignal()
	if err != nil {
		log.Error("create reach signal subscriber fail, err: %s", err.Error())
	} else {
		log.Info("create reach signal subscriber pass")
	}
	//地面材质
	err = s.createFloorMaterial()
	if err != nil {
		log.Error("create floor material subscriber fail, err: %s", err.Error())
	} else {
		log.Info("create floor material subscriber pass")
	}

	//motor control publisher
	err = s.createMotorPublisher()
	if err != nil {
		log.Error("create motor publisher fail, err: %s", err.Error())
	} else {
		log.Info("create motor publisher pass")
	}

	//motor speed control publisher
	err = s.createMotorSpeedPublisher()
	if err != nil {
		log.Error("create motor speed publisher fail, err: %s", err.Error())
	} else {
		log.Info("create motor speed publisher pass")
	}

	//extend motor control publisher
	err = s.createExtendMotorPublisher()
	if err != nil {
		log.Error("create extend motor publisher fail, err: %s", err.Error())
	} else {
		log.Info("create extend motor publisher pass")
	}
	//light control publisher
	err = s.createLightPublisher()
	if err != nil {
		log.Error("create light publisher fail, err: %s", err.Error())
	} else {
		log.Info("create light publisher pass")
	}
	//sensor control
	err = s.createSensorPublisher()
	if err != nil {
		log.Error("create sensor publisher fail, err: %s", err.Error())
	} else {
		log.Info("create sensor publisher pass")
	}
	//mcu poweroff
	err = s.createMCUPowerOffPublisher()
	if err != nil {
		log.Error("create mcu power off publisher fail, err: %s", err.Error())
	} else {
		log.Info("create mcu power off publisher pass")
	}
	return nil
}

func (s *IotServer) Release() {
	for topicName, publisher := range s.publisherMap {
		log.Info("close topic %s publisher", topicName)
		publisher.Close()
	}
	log.Info("close ros node")
	s.node.Close()
}

/**
 * @brief: 	监听ros状态
 * @author: haitao.lin
 */
func (s *IotServer) listenROSState() {
	for true {
		c := apimaster.NewClient(fmt.Sprintf("%s:11311", s.MachineHost), "tool_heart")
		uri, err := c.GetURI() //获取不到 master uri，表示远程ros已经关闭，或者已经断开
		if err != nil {
			log.Error("get uri fail, %s", err.Error())
			s.writeROSState(1, err.Error()) //1表示断开
			s.node.Close()
			break
		}
		log.Info(uri)
		time.Sleep(10 * time.Second)
	}
}

func (s *IotServer) createPublisher(topicName string) error {
	publisher, err := goroslib.NewPublisher(goroslib.PublisherConf{
		Node:  s.node,
		Topic: topicName,
		Msg:   &std_msgs.String{},
		Latch: false,
	})
	if err != nil {
		return err
	}
	s.publisherMap[topicName] = publisher
	return nil
}

func (s *IotServer) createMotorPublisher() error {
	publisher, err := goroslib.NewPublisher(goroslib.PublisherConf{
		Node:  s.node,
		Topic: "/serial_com_driver/control_clean_device",
		Msg:   &common_msg.ControlCleanDevice{},
		Latch: false,
	})
	if err != nil {
		return err
	}
	s.publisherMap["motor_control"] = publisher
	return nil
}

func (s *IotServer) createMotorSpeedPublisher() error {
	publisher, err := goroslib.NewPublisher(goroslib.PublisherConf{
		Node:  s.node,
		Topic: "/serial_com_driver/control_clean_device_speed",
		Msg:   &common_msg.ControlCleanDeviceSpeed{},
		Latch: true,
	})
	if err != nil {
		return err
	}
	s.publisherMap["motor_speed_control"] = publisher
	return nil
}

func (s *IotServer) createExtendMotorPublisher() error {
	publisher, err := goroslib.NewPublisher(goroslib.PublisherConf{
		Node:  s.node,
		Topic: "/serial_com_driver/control_extend_device",
		Msg:   &common_msg.ControlExtendDevice{},
		Latch: false,
	})
	if err != nil {
		return err
	}
	s.publisherMap["device_extend"] = publisher
	return nil
}

func (s *IotServer) createLightPublisher() error {
	publisher, err := goroslib.NewPublisher(goroslib.PublisherConf{
		Node:  s.node,
		Topic: "/serial_com_driver/control_light",
		Msg:   &common_msg.ControlLight{},
		Latch: false,
	})
	if err != nil {
		return err
	}
	s.publisherMap["light_control"] = publisher
	return nil
}

func (s *IotServer) createSensorPublisher() error {
	publisher, err := goroslib.NewPublisher(goroslib.PublisherConf{
		Node:  s.node,
		Topic: "/serial_com_driver/control_sensor_switch",
		Msg:   &common_msg.ControlSensorSwitch{},
		Latch: false,
	})
	if err != nil {
		return err
	}
	s.publisherMap["sensor_control"] = publisher
	return nil
}

func (s *IotServer) createMCUPowerOffPublisher() error {
	publisher, err := goroslib.NewPublisher(goroslib.PublisherConf{
		Node:  s.node,
		Topic: "/serial_com_driver/control_power",
		Msg:   &common_msg.ControlPower{},
		Latch: false,
	})
	if err != nil {
		return err
	}
	s.publisherMap["mcu_poweroff"] = publisher
	return nil
}

func (s *IotServer) createSubscriber(topicName string) error {
	subscriber, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:  s.node,
		Topic: topicName,
		Callback: func(msg *std_msgs.String) {
			s.writeIOTData(topicName, msg.Data)
		},
		QueueSize: 10,
	})
	if err != nil {
		return err
	}
	s.subscriberMap[topicName] = subscriber
	log.Info("create topic %s subscriber pass", topicName)
	return nil
}

/**
 * @brief: 	订阅下视信号
 * @author: haitao.lin
 */
func (s *IotServer) createDownViewSignal() error {
	topicName := "/serial_com_driver/downview_signal"
	subscriber, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:  s.node,
		Topic: topicName,
		Callback: func(msg *common_msg.DownViewSignal) {
			s.sendTopicData("down_view_signal", DownViewSignalData{
				LeftFront:  msg.LeftFront,
				RightFront: msg.RightFront,
				LeftAfter:  msg.LeftAfter,
				RightAfter: msg.RightAfter,
			})
		},
		QueueSize: 10,
	})
	if err != nil {
		return err
	}
	s.subscriberMap[topicName] = subscriber
	log.Info("create topic %s subscriber pass", topicName)
	return nil
}

/**
 * @brief: 	订阅下视编码值
 * @author: haitao.lin
 */
func (s *IotServer) createDownViewCode() error {
	topicName := "/serial_com_driver/down_view_code"
	subscriber, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:  s.node,
		Topic: topicName,
		Callback: func(msg *common_msg.DownViewCode) {
			s.sendTopicData("down_view_code", DownViewCodeData{
				LeftFront:  msg.LeftFront,
				RightFront: msg.RightFront,
				LeftAfter:  msg.LeftMiddle,
				RightAfter: msg.RightMiddle,
			})
		},
		QueueSize: 10,
	})
	if err != nil {
		return err
	}
	s.subscriberMap[topicName] = subscriber
	log.Info("create topic %s subscriber pass", topicName)
	return nil
}

/**
 * @brief: 	订阅撞板信号
 * @author: haitao.lin
 */
func (s *IotServer) createBumpSignal() error {
	topicName := "/serial_com_driver/bump_signal"
	subscriber, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:  s.node,
		Topic: topicName,
		Callback: func(msg *common_msg.BumpSignal) {
			s.sendTopicData("bump_signal", BumpSignalData{
				Left:  msg.Left,
				Right: msg.Right,
			})
		},
		QueueSize: 10,
	})
	if err != nil {
		return err
	}
	s.subscriberMap[topicName] = subscriber
	log.Info("create topic %s subscriber pass", topicName)
	return nil
}

/**
 * @brief:  悬空/跌落
 * @author: haitao.lin
 */
func (s *IotServer) createHanging() error {
	topicName := "/serial_com_driver/drop_signal"
	subscriber, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:  s.node,
		Topic: topicName,
		Callback: func(msg *common_msg.DropSignal) {
			s.sendTopicData("hanging_signal", DropSignalData{
				Left:  msg.Left,
				Right: msg.Right,
			})
		},
		QueueSize: 10,
	})
	if err != nil {
		return err
	}
	s.subscriberMap[topicName] = subscriber
	log.Info("create sensor subscriber %s subscriber pass", topicName)
	return nil
}

/**
 * @brief:  外设在位信号
 * @author: haitao.lin
 */
func (s *IotServer) createInstallSignal() error {
	topicName := "/serial_com_driver/install_signal"
	subscriber, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:  s.node,
		Topic: topicName,
		Callback: func(msg *common_msg.InstallSignal) {
			s.sendTopicData("install_signal", InstallSignal{
				DustBox:  msg.DustBox,
				WaterBox: msg.WaterBox,
				LeftMop:  msg.LeftMop,
				RightMop: msg.RightMop,
			})
		},
		QueueSize: 10,
	})
	if err != nil {
		return err
	}
	s.subscriberMap[topicName] = subscriber
	log.Info("create sensor subscriber %s subscriber pass", topicName)
	return nil
}

/**
 * @brief:  外设到位信号
 * @author: haitao.lin
 */
func (s *IotServer) createReachSignal() error {
	topicName := "/serial_com_driver/reach_signal"
	subscriber, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:  s.node,
		Topic: topicName,
		Callback: func(msg *common_msg.ReachSignal) {
			s.sendTopicData("reach_signal", ReachSignal{
				RollBrushUp:  msg.RollBrushUp,
				EdgeBrushOut: msg.EdgeBrushOut,
				EdgeBrushIn:  msg.EdgeBrushIn,
				MopOut:       msg.MopOut,
				MopIn:        msg.MopIn,
				LeftMopUp:    msg.LeftMopUp,
				RightMopUp:   msg.RightMopUp,
			})
		},
		QueueSize: 10,
	})
	if err != nil {
		return err
	}
	s.subscriberMap[topicName] = subscriber
	log.Info("create sensor subscriber %s subscriber pass", topicName)
	return nil
}

/**
 * @brief:  地面材质
 * @author: haitao.lin
 */
func (s *IotServer) createFloorMaterial() error {
	topicName := "/serial_com_driver/floor_material"
	subscriber, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:  s.node,
		Topic: topicName,
		Callback: func(msg *common_msg.FloorMaterial) {
			s.sendTopicData("floor_material", FloorMaterial{Type: msg.Value})
		},
		QueueSize: 10,
	})
	if err != nil {
		return err
	}
	s.subscriberMap[topicName] = subscriber
	log.Info("create sensor subscriber %s subscriber pass", topicName)
	return nil
}

/**
 * @brief:  地毯类型
 * @author: haitao.lin
 */
func (s *IotServer) createCarpetType() error {
	topicName := "/serial_com_driver/carpet_type"
	subscriber, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:  s.node,
		Topic: topicName,
		Callback: func(msg *common_msg.CarpetType) {
			s.sendTopicData("carpet_type", CarpetType{Type: msg.Value})
		},
		QueueSize: 10,
	})
	if err != nil {
		return err
	}
	s.subscriberMap[topicName] = subscriber
	log.Info("create sensor subscriber %s subscriber pass", topicName)
	return nil
}
